#include "livewire.h"
#include "imgProcess.h"
#include "math.h"
    int xoff[8] = { -1, 0, 1, -1, 1, -1, 0, 1 };
    int yoff[8] = { -1, -1, -1, 0, 0, 1, 1, 1 };
    float dWeight[8] = { 1, 0.7, 1, 0.7, 0.7, 1, 0.7, 1 };
LiveWire::LiveWire()
{
    _isCalPath = false;
    _pathLength = 0;


}

bool LiveWire::GetPath(int x, int y)
{
    _pathLength = 0;

    int _x = x;
    int _y = y;

    pathPoint[_pathLength].x = x;
    pathPoint[_pathLength].y = y;

    while (iMapX(_x, _y) != 0 || iMapY(_x, _y) != 0)
    {
        int tempindex = _x;
        
        _x += iMapX(tempindex, _y);
        _y += iMapY(tempindex, _y);
        
        _pathLength++;

        pathPoint[_pathLength].x = _x;
        pathPoint[_pathLength].y = _y;
    }

    for (int i = 0; i < (_pathLength + 1) / 2; i++)
    {
        cimg_library::POINT tempPoint;
        tempPoint = pathPoint[i];
        pathPoint[i] = pathPoint[_pathLength - i];
        pathPoint[_pathLength - i] = tempPoint;
    }

    return false;
}

bool LiveWire::GetCost(CImg<unsigned char> src, CImg<double> *dst, float dWZ, float dWg)
{
    CImg<double> temp(src);

    CImgList<double> gradlist = temp.get_gradient();
    CImg<double> grad = sqrt(gradlist[0].get_mul(gradlist[0]) + gradlist[1].get_mul(gradlist[1]));
    double maxGrad = grad.max();
    grad = 1 - grad / maxGrad;

    CImg<double> lFz = 1 - sobel(src) / 255;

    *dst = dWZ*lFz + dWg*grad;
    _cost = *dst;


    lE = _cost;
    lE.fill(0);

    iPX = CImg<int>(_cost._width, _cost._height, _cost._depth, _cost._spectrum, 0);
    iPY = iPX;

    iNX = CImg<int>(_cost._width, _cost._height, 8, _cost._spectrum, 0);
    iNY = iNX;

    cimg_forXYZ(iNX, X, Y, Z)
    {
        iNX(X, Y, Z) = X + xoff[Z];
        iNY(X, Y, Z) = Y + yoff[Z];
    }
    return true;
}

bool LiveWire::CalculatePathMap(CImg<double> cost, int x, int y, int radius)
{

    const int iLISTMAXLENGTH = 10000;
    int iNPixelsToProcess = cimg::PI*radius*radius > cost._width*cost._height ? cost._width*cost._height : cimg::PI*radius*radius;
    int iNPixelsProcessed = 0;

    int iLI[iLISTMAXLENGTH][3];
    double dLG[iLISTMAXLENGTH];
    int iLInd = 0;
    for (int i = 0; i < iLISTMAXLENGTH; i++)
    {
        for (int j = 0; j < 3; j++)
        {
            iLI[i][j] = 0;
        }
        dLG[i] = 0;
    }


    lE.fill(0);
    iPX.fill(0);
    iPY.fill(0);



    //iNX.display();
    //iLInd += 1;
    iLI[iLInd][0] = x;
    iLI[iLInd][1] = y;
    iLI[iLInd][2] = y*cost._width + x;

    dLG[iLInd] = 0;

    while (iLInd >= 0 && iNPixelsProcessed < iNPixelsToProcess)
    {
        // -------------------------------------------------------------------- -
        //    Determine pixel q in list with minimal cost and remove from active
        //    list.Mark q as processed.
        double min_dLG = dLG[0];
        int iInd = 0;
        {//find min from dLG;
            for (int i = 0; i <= iLInd; i++)
            {
                if (min_dLG > dLG[i])
                {
                    min_dLG = dLG[i];
                    iInd = i;
                }
            }
        }

        int iQI[3] = { iLI[iInd][0], iLI[iInd][1], iLI[iInd][2] };
        double dQG = dLG[iInd];

        iLI[iInd][0] = iLI[iLInd][0];
        iLI[iInd][1] = iLI[iLInd][1];
        iLI[iInd][2] = iLI[iLInd][2];

        dLG[iInd] = dLG[iLInd];

        iLInd -= 1;

        lE(iQI[0], iQI[1]) = 1;
        //---------------------------------------------------------------------

        // -------------------------------------------------------------------- -
        // Generate neighbourhood of q
        CImg<int> iNeighbour = CImg<int>(8, 4, 1, 1, 0);
        cimg_forX(iNeighbour, X)
        {
            iNeighbour(X, 0) = iNX(iQI[0], iQI[1], X);
            iNeighbour(X, 1) = iNY(iQI[0], iQI[1], X);
            iNeighbour(X, 2) = xoff[7 - X];
            iNeighbour(X, 3) = yoff[7 - X];
        }
        //iNeighbour.display();
        // -------------------------------------------------------------------- -

        for (int i = 0; i < 8; i++)
        {
            if (iNeighbour(i, 1) >= 0 && iNeighbour(i, 1) < cost._height&&iNeighbour(i, 0) >= 0 && iNeighbour(i, 0) < cost._width)
            {

                if (!lE(iNeighbour(i, 0), iNeighbour(i, 1)))
                {
                    int iR[4] = { iNeighbour(i, 0), iNeighbour(i, 1), iNeighbour(i, 2), iNeighbour(i, 3) };

                    int iLinInd = iR[1] * cost._width + iR[0];
                    
                    double dThisG = dQG + cost(iR[0], iR[1])*dWeight[i];

                    bool find_ilinInd_from_iLI = false;
                    for (int j = 0; j < iLInd; j++)
                    {
                        if (iLI[j][2] == iLinInd)
                        {
                            iInd = j;
                            find_ilinInd_from_iLI = true;
                            break;
                        }
                    }

                    if (find_ilinInd_from_iLI)
                    {
                        if (dThisG < dLG[iInd])
                        {
                            dLG[iInd] = dThisG;
                            iPX(iR[0], iR[1]) = iR[2];
                            iPY(iR[0], iR[1]) = iR[3];
                        }
                    }
                    else
                    {
                        iLInd += 1;
                        iLI[iLInd][0] = iR[0];
                        iLI[iLInd][1] = iR[1];
                        iLI[iLInd][2] = iLinInd;

                        dLG[iLInd] = dThisG;
                        iPX(iR[0], iR[1]) = iR[2];
                        iPY(iR[0], iR[1]) = iR[3];

                    }
                }
            }
        }

        iNPixelsProcessed = iNPixelsProcessed + 1;
    }

    iMapX = iPX;
    iMapY = iPY;
    _isCalPath = true;
    return true;
}

bool LiveWire::GetIdealAnchor()
{
    return false;
}
